addpath('./build')

% load('a_opt');
% load('x_opt');
% x0 = x_opt;
% a0 = a_opt;

% p0 : [stance/nonstance flat, ...
%       initial hip position (unused), ...
%       x-location of stance foot];
%p0 = [1, -0.178825138445300, 0];
d0 = 0;

numsteps = 10;

ic = [-0.399304397122687
   0.329870352753882
   0.139253019211256
  -0.316094874566801
   0.160534051402593
  -1.613330187248071
   0.386695879907189
   1.462733580986071
   2.088313203546850
  -4.615392284425383];

% a0=[0.599905758858752                   0                   0                   0                   0
%    0.088319784190409   7.458328891437005   0.086824594333935  -2.527010921765337   0.185552626240693
%   -0.182019683942709  11.618976290206323   0.150902483808764   4.214271216226255   0.316325226650619
%   -0.347269593531121  -9.702266322079611  -0.134652933292173  -0.765456083469425   0.650844686308114
%    0.000000000079389  10.411022920936052  -0.000000001367527   3.292172758159841   0.074934499244602];
% 
% access_A_matrix(a0);

lsiObj = LSysInfo();

[xr, ~] = reset_map(ic, d0);
data = hybrid_flow(.95*xr, d0, numsteps, lsiObj);

T = cat(2, data.time{:});
u = nan(5, length(T));

k = 0;
for i = 1:length(data.time)
    t = data.time{i};
    x = data.integral_curves{i};
    
    for j = 1:length(t)
        k = k + 1;
        u(:, k) = lsiObj.getControl(...
            t(j), ...
            x(:, j));
    end
end

plot(T, u)
